#include <ros/ros.h>
#include "mcu_correspond/mcu_data_subscriber.h"
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <cmath>
#include <locale>

class ColorDetector {
private:
    ros::NodeHandle nh_;
    ros::Subscriber image_sub_;
    ros::Publisher image_pub_;
    McuDataSubscriber mcu_data_;
    ros::Rate publish_rate_;
    ros::Time last_publish_time_;

public:
    ColorDetector(ros::NodeHandle& nh) : nh_(nh), mcu_data_(nh), publish_rate_(1.0) {
        // Subscribe to the camera image topic
        image_sub_ = nh_.subscribe("/usb_cam_2060s/image_raw", 1, &ColorDetector::imageCallback, this);
        // Publish the processed image
        image_pub_ = nh_.advertise<sensor_msgs::Image>("/usb_cam_2060s/img_processed", 1);
        last_publish_time_ = ros::Time::now();
    }
    
    void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
        cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        cv::Mat image;
        cv::resize(cv_ptr->image, image, cv::Size(640, 360));

        // ROI
        cv::Rect roi;
        cv::Mat roi_image;
        int height = image.rows;
        int width = image.cols;
        int roi_width= width / 7;
        int roi_x = width * 3 / 7; // 从x图像1/7处开始
        int roi_height = height / 5;
        int roi_y = height * 2 / 5; // 从y图像1/5处开始
        roi = cv::Rect(roi_x, roi_y, roi_width, roi_height);
        roi_image = image(roi).clone();

        // 2G - B - R
        cv::Mat green_mask = cv::Mat::zeros(roi_image.size(), CV_8UC1);
        for (int y = 0; y < roi_image.rows; y++) 
        {
            for (int x = 0; x < roi_image.cols; x++) 
            {
                cv::Vec3b pixel = roi_image.at<cv::Vec3b>(y, x);
                int value = (pixel[1]*2-pixel[0]-pixel[0]-5)*5;
                value = std::min(std::max(value, 0), 255); // 确保值在0-255范围内
                green_mask.at<uchar>(y, x) = value;
            }
        }

        cv::imshow("Green Mask", green_mask);
        cv::waitKey(1);

        // 发布处理后的图像
        sensor_msgs::ImagePtr processed_msg = cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::MONO8, green_mask).toImageMsg();
        processed_msg->header.stamp = ros::Time::now();
        image_pub_.publish(processed_msg);

        if (ros::Time::now() - last_publish_time_ >= ros::Duration(0.3)) {
            int is_green = (cv::mean(green_mask)[0] > 35) ? 1 : 0; // 判断是否有绿色区域    
            mcu_data_.publishDoubleData("is_green", is_green);
            ROS_INFO("Published is_green: %d", is_green);
            last_publish_time_ = ros::Time::now();
        }

    }
};

int main(int argc, char** argv) {
    std::setlocale(LC_ALL, "zh_CN.UTF-8");
    ros::init(argc, argv, "g2021down");
    ros::NodeHandle nh;
    
    // Create color detector
    ColorDetector detector(nh);
    
    ros::spin();
    return 0;
}
